#include "main.h"
#include "car.h"
#include "fifgray.h"

#define DELAY (16000000)

// 小车的状态
static enum {
    car_state_stop,
    car_state_question_1_run,
    car_state_question_1_plan_2_run,
    car_state_question_track,
    car_state_question_2_run,
} g_car_state = car_state_stop;

// 前置函数声明

// 第一题
void on_question_1(void);
// 第二题
void on_question_2(void);
// 第三题
void on_question_3(void);
// 第四题
void on_question_4(void);
// 小车停止
void car_stop(void);
// 小车前进
void car_forward(int32_t speed);
// 小车转angle度，左转是负，右转是正
void car_turn_angle(float angle);
// 速度环控制
void car_speed_control(void);
// 角度环控制
void car_angle_control(void);

// 调试串口
// 电脑RX->A0
// 电脑TX->A1

// led灯，低电平亮
#define led_on() DL_GPIO_clearPins(GPIO_GRP_MISC_PIN_LED_PORT,GPIO_GRP_MISC_PIN_LED_PIN)//CLK
#define led_off() DL_GPIO_setPins(GPIO_GRP_MISC_PIN_LED_PORT,GPIO_GRP_MISC_PIN_LED_PIN)


// 蜂鸣器，板载的PA27，高电平有效
beep_t g_beep = {.port  = GPIO_GRP_MISC_PIN_BEEP_PORT,
                 .pin   = GPIO_GRP_MISC_PIN_BEEP_PIN,
                 .valid = VALID_ON_HIGH_LEVEL};
// 前左轮电机 pwmA->PA5,in1->PB14,in2->PB16
motor_t g_motor_fl = {.timer       = PWM_FRONT_MOTOR_INST,
                      .channel     = GPIO_PWM_FRONT_MOTOR_C0_IDX,
                      .max_peiod   = 3200,
                      .current_val = 0,
                      .port_in1    = GPIO_GRP_MOTOR_PORT,
                      .pin_in1     = GPIO_GRP_MOTOR_PIN_FRONT_LEFT_A_PIN,
                      .port_in2    = GPIO_GRP_MOTOR_PORT,
                      .pin_in2     = GPIO_GRP_MOTOR_PIN_FRONT_LEFT_B_PIN,
                      .rotate      = MOTOR_DIRECTION_COROTATION_IN1};

// 前右轮电机 pwmB->PA6,in1->PB13,in2->PB15
motor_t g_motor_fr = {.timer       = PWM_FRONT_MOTOR_INST,
                      .channel     = GPIO_PWM_FRONT_MOTOR_C1_IDX,
                      .max_peiod   = 3200,
                      .current_val = 0,
                      .port_in1    = GPIO_GRP_MOTOR_PORT,
                      .pin_in1     = GPIO_GRP_MOTOR_PIN_FRONT_RIGHT_A_PIN,
                      .port_in2    = GPIO_GRP_MOTOR_PORT,
                      .pin_in2     = GPIO_GRP_MOTOR_PIN_FRONT_RIGHT_B_PIN,
                      .rotate      = MOTOR_DIRECTION_COROTATION_IN1};

// 编码器，e1a->B0,e1b->B1
motor_encoder_t g_encoder_e1 = {.port_a = GPIO_GRP_ENCODER_PORT,
                                .pin_a  = GPIO_GRP_ENCODER_PIN_E1A_PIN,
                                .port_b = GPIO_GRP_ENCODER_PORT,
                                .pin_b  = GPIO_GRP_ENCODER_PIN_E1B_PIN,
                                .count  = 0};

// 编码器，e2a->B2,e2b->B3
motor_encoder_t g_encoder_e2 = {.port_a = GPIO_GRP_ENCODER_PORT,
                                .pin_a  = GPIO_GRP_ENCODER_PIN_E2A_PIN,
                                .port_b = GPIO_GRP_ENCODER_PORT,
                                .pin_b  = GPIO_GRP_ENCODER_PIN_E2B_PIN,
                                .count  = 0};

// 陀螺仪数据
float g_acc_x, g_acc_y, g_acc_z;         // 加速度
float g_gyro_x, g_gyro_y, g_gyro_z;      // 角速度
float g_angle_x, g_angle_y, g_angle_z;   // 角度

PidTypeDef g_pid1;        // 左前轮速度的目标值
PidTypeDef g_pid2;        // 右前轮速度的目标值
PidTypeDef g_pid_angle;   // 角度的pid

// 速度为单位时间内编码器的脉冲数
int32_t g_target_speed1;
int32_t g_target_speed2;
int32_t g_target_angle;

float        L_Kp, L_Ki, L_Kd, R_Kp, R_Ki, R_Kd;
double       SumError = 0, LastError = 0;
int          dError    = 0;
double       RSumError = 0, RLastError = 0;
int          RdError = 0;
unsigned int outpwml, outpwmr, setspeedr, setspeedl;

uint32_t leftroad, rightroad;
uint32_t setleftroad, setrightroad;

float  Road_L_Kp, Road_L_Ki, Road_L_Kd, Road_R_Kp, Road_R_Ki, Road_R_Kd;
double Road_SumError = 0, Road_LastError = 0;
int    Road_dError    = 0;
double Road_RSumError = 0, Road_RLastError = 0;
int    Road_RdError = 0;
                                
// 根据占空比设置左前轮目标速度
inline void set_target1_speed(int32_t speed)
{
    g_target_speed1 = speed;
    //    char buf[32];
    //    sprintf(buf, "res1=%.3f", g_target_speed1);
    //    OLED_ShowString(2, 1, buf);
}
// 根据占空比设置右前轮目标速度
inline void set_target2_speed(int32_t speed)
{
    g_target_speed2 = speed;
}
// 设置目标角度
inline void set_target_angle(float angle)
{
    g_target_angle = angle;
}
// 左转，要求右轮速度大于左轮速度
inline void car_turn_left(int32_t leftspeed, int32_t rightspeed)
{
    g_target_speed1 = leftspeed;
    g_target_speed2 = rightspeed;
}
// 右转，要求左轮速度大于右轮速度
inline void car_turn_right(int32_t leftspeed, int32_t rightspeed)
{
    g_target_speed1 = leftspeed;
    g_target_speed2 = rightspeed;
}
// 主函数
int main(void)
{
    // 为了防止出现进不了中断，在一开始先清除标志位
    NVIC_ClearPendingIRQ(GPIO_GRP_ENCODER_INT_IRQN);
    NVIC_ClearPendingIRQ(UART_DEBUG_INST_INT_IRQN);
    NVIC_ClearPendingIRQ(UART_JY61P_INST_INT_IRQN);
    NVIC_ClearPendingIRQ(TIMER_0_INST_INT_IRQN);

    // 芯片资源初始化,由SysConfig配置软件自动生成
    SYSCFG_DL_init();

    
    
    // 开启中断
    NVIC_EnableIRQ(GPIO_GRP_ENCODER_INT_IRQN);
    NVIC_EnableIRQ(UART_DEBUG_INST_INT_IRQN);
    NVIC_EnableIRQ(UART_JY61P_INST_INT_IRQN);   // 开jy61p串口的接收中断
    NVIC_EnableIRQ(TIMER_0_INST_INT_IRQN);      // 定时器中断

    // 外设初始化
    debug_init(UART_DEBUG_INST);
    // 输出调试信息
    DEBUG_INFO("start");

    // jy61p TX->A13
    jy61p_init(UART_JY61P_INST);
    DEBUG_INFO("jy61p_init");
    // 尝试读取数据，如果都为0，那么判定为
    // jy61p_get_angle(&g_angle_x, &g_angle_y, &g_angle_z);

    // oled初始化
    OLED_Init();
    OLED_Clear();
    DEBUG_INFO("OLED_Init");

    // 按键初始化
    key_init();
    DEBUG_INFO("key_init");

    // 添加编码器
    motor_encoder_add(&g_encoder_e1);
    motor_encoder_add(&g_encoder_e2);
    DEBUG_INFO("motor_encoder_add");

    // 初始化pid
    PID_Init(&g_pid1, PID_DELTA, PID1_KP, PID1_KI, PID1_KD, PID1_MAX_OUT, PID1_MAX_IOUT);
    PID_Init(&g_pid2, PID_DELTA, PID2_KP, PID2_KI, PID2_KD, PID2_MAX_OUT, PID2_MAX_IOUT);
    PID_Init(&g_pid_angle, PID_DELTA, PIDA_KP, PIDA_KI, PIDA_KD, PIDA_MAX_OUT, PIDA_MAX_IOUT);
    DEBUG_INFO("pid_init");

    // 获取按键键值
    // 阻塞式读取按键
    int32_t key      = 0;
    int32_t question = 1;
    char    buf[32];
    sprintf(buf, "key=%d,question=%d", key, question);
    OLED_ShowString(0, 0, (u8 *)buf, 8);
    while (1) {
        buf[0] = '\0';
        //        jy61p_get_angle(&g_angle_x, &g_angle_y, &g_angle_z);
        //        sprintf(buf, "ax=%.3f", g_angle_x);
        //        OLED_ShowString(2, 1, buf);
        //        sprintf(buf, "ay=%.3f", g_angle_y);
        //        OLED_ShowString(3, 1, buf);
        //        sprintf(buf, "az=%.3f", g_angle_z);
        //        OLED_ShowString(4, 1, buf);

        uint8_t gray_arr[5];
        // 读取灰度的数据
        //stats_out(GPIOA, Front_one, gray_arr, 5);
        sprintf(buf, "a22=%d,a23=%d", gray_arr[0], gray_arr[1]);
        OLED_ShowString(2, 1, (u8 *)buf, 8);
        sprintf(buf, "a24=%d,a25=%d", gray_arr[2], gray_arr[3]);
        OLED_ShowString(3, 1, (u8 *)buf, 8);
        sprintf(buf, "a26=%d", gray_arr[4]);
        OLED_ShowString(4, 1, (u8 *)buf, 8);

        key = key_get_value();
        sprintf(buf, "key=%d,question=%d", key, question);
        OLED_ShowString(1, 1, (u8 *)buf, 8);
        if (key == KEY_VALUE_1) {
            question = question % 4 + 1;
            // DEBUG_INFO("key=%d", key);
            // DEBUG_INFO("question=%d", question);
            delay_ms(100);
        }
        if (key == KEY_VALUE_2) { break; }
        if (key == KEY_VALUE_3) {
            // jy61p_clear_yaw();
        }
        delay_ms(100);
    }
    // DEBUG_INFO("final question=%d", question);
    sprintf(buf, "final question=%d", question);
    OLED_ShowString(1, 1, (u8 *)buf, 16);

    // 开电机的定时器
    DL_TimerG_startCounter(PWM_FRONT_MOTOR_INST);

    // 开pid控制的定时器
    DL_TimerG_startCounter(TIMER_0_INST);

    switch (question) {
    case 1: on_question_1(); break;
    case 2: on_question_2(); break;
    case 3: on_question_3(); break;
    case 4: on_question_4(); break;
    }
}

uint8_t Flag_PID     = 0;
uint8_t Flag_Encode  = 0;
uint8_t beep_on_flag = 0;   //蜂鸣器和灯光是否工作

// pid控制定时器，10ms进入一次
void TIMER_0_INST_IRQHandler()
{
    static unsigned int beeptimecount = 0;
    // DEBUG_INFO("TIMER_0_INST_IRQHandler");
    switch (DL_TimerA_getPendingInterrupt(TIMER_0_INST)) {
    case DL_TIMERA_IIDX_ZERO:
    {
        switch (g_car_state) {
        case car_state_question_1_plan_2_run:
        {
            // 获取当前的计数值
            int32_t lcount = g_encoder_e1.count;
            int32_t rcount = g_encoder_e2.count;
            // 清空计数值
            g_encoder_e1.count = 0;
            g_encoder_e2.count = 0;
            // 累加所走过的路程
            leftroad += lcount;
            rightroad += rcount;
            
            Flag_PID++;   // PID控制标志加一
            Flag_Encode = 1;
            if (beep_on_flag == 1) {
                beeptimecount++;
                // 如果是120ms以内，那么间隔20ms切换一下
                if (beeptimecount <= 2) {
                    led_on();
                    beep_on(&g_beep);
                }
                else if (beeptimecount <= 4) {
                    led_off();
                    beep_off(&g_beep);
                }
                else if (beeptimecount <= 6) {
                    led_on();
                    beep_on(&g_beep);
                }
                else if (beeptimecount <= 8) {
                    led_off();
                    beep_off(&g_beep);
                }
                else if (beeptimecount <= 10) {
                    led_on();
                    beep_on(&g_beep);
                }
                else if (beeptimecount <= 12) {
                    led_off();
                    beep_off(&g_beep);
                }
                else
                    beep_on_flag = 0;
            }
            else {
                // 关闭蜂鸣器和灯光
                led_off();
                beep_off(&g_beep);
            }

        } break;
        case car_state_question_1_run: car_speed_control(); break;
        default: break;
        }
    } break;
    default: break;
    }
}

int32_t detect_black_lines(void)
{
    uint8_t gray_arr[5];
    // 读取灰度的数据
    //stats_out(GPIOA, Front_one, gray_arr, 5);
    // 判断是否有黑线
    for (int i = 0; i < 5; i++) {
        if (gray_arr[i] == 1) { return 1; }
    }
    return 0;   // 没有黑线
}

void track(void)
{
    // 寻迹
    while (1) {
        uint8_t gray_arr[5];
        // 读取灰度的数据
        //stats_out(GPIOA, Front_one, gray_arr, 5);
        
        // todo
        // 获取灰度的数据
        if(gray_arr[0] == 1){
            // 调整车左右轮子的速度
            car_turn_left(40, 30);
        }
        if(gray_arr[1] == 1){
            // 调整车左右轮子的速度
            car_turn_left(40, 30);
        }
        if(gray_arr[2] == 1){
            // 调整车左右轮子的速度
            car_turn_left(40, 30);
        }
        if(gray_arr[3] == 1){
            // 调整车左右轮子的速度
            car_turn_left(40, 30);
        }
        if(gray_arr[4] == 1){
            // 调整车左右轮子的速度
            car_turn_left(40, 30);
        }

        // 发现没有线了
        if (detect_black_lines() == 0) {
            // 如果检测不到黑线
            // 开灯光和蜂鸣器响应
            beep_on_flag = 1;
            break;
        }
    }
}

void L_Pid_Ctrl(unsigned int Target, unsigned int Real)   // PID速度控制函数
{
    int   Error = 0;   //本次差值，上次差值
    float j     = 0.0, i;

    if (Target == 0) {
        LastError = 0;
        dError    = 0;
        SumError  = 0;

        outpwml = 0;
        motor_set_compare_val(&g_motor_fl, outpwml);
        return;
    }

    Error = (Target - Real);      //本次偏差
    if (SumError * L_Ki > 1600)   //计算积分项
    {
        SumError = 1600;
        SumError = SumError / L_Ki;
    }   // 输出到最大值后，不再累计偏差，抗积分饱各
    else
        SumError += Error;

    dError    = Error - LastError;   //微分项，偏差的变化
    LastError = Error;               //记录本次偏差，用于下次计算

    i = L_Kp;
    j = Error * i;
    i = L_Ki;
    j = j + SumError * i;
    i = L_Kd;
    j = j + dError * i;

    if (j > 1600)
        outpwml = 1600 + 1;
    else if (j < 1)
        outpwml = 1;
    else
        outpwml = j;
    motor_set_compare_val(&g_motor_fl, outpwml);
}

void R_Pid_Ctrl(unsigned int Target, unsigned int Real)   // PID速度控制函数
{
    int   RError = 0;   //本次差值，上次差值
    float j      = 0.0, i;

    if (Target == 0) {
        RLastError = 0;
        RdError    = 0;
        RSumError  = 0;

        outpwmr = 0;
        motor_set_compare_val(&g_motor_fr, outpwml);
        return;
    }

    RError = (Target - Real);      //本次偏差
    if (RSumError * R_Ki > 1600)   //计算积分项
    {
        RSumError = 1600;
        RSumError = RSumError / L_Ki;
    }   // 输出到最大值后，不再累计偏差，抗积分饱各
    else
        RSumError += RError;

    RdError    = RError - RLastError;   //微分项，偏差的变化
    RLastError = RError;                //记录本次偏差，用于下次计算

    i = R_Kp;
    j = RError * i;
    i = R_Ki;
    j = j + RSumError * i;
    i = R_Kd;
    j = j + RdError * i;

    if (j > 1600)
        outpwmr = 1600 + 1;
    else if (j < 1)
        outpwmr = 1;
    else
        outpwmr = j;

    motor_set_compare_val(&g_motor_fr, outpwml);
}
int32_t SPEED_RIGHT, SPEED_LEFT;   // 这两个应该是编码器脉冲数为单位，目前还没有写
// 第一题
void on_question_1(void)
{
    // 基于纯编码器的实现，一点一点挪动，然后修正
    g_car_state = car_state_question_1_plan_2_run;   // 切换到方案2的中断代码

    // 设置一个初始速度
    car_forward(80);

    // 理论上走完从A到B所需的编码器数值
    uint32_t left_road  = 5000; // unused
    uint32_t right_road = 5000; // unused
    while (1) {
        // 读取灰度的值，并且通过灰度来判断是否检测到黑线
        if (detect_black_lines() == 1) {
            // 如果检测到黑线，先关闭电机（停车）
            motor_off(&g_motor_fl);
            motor_off(&g_motor_fr);
            // 开灯光和蜂鸣器响应
            beep_on_flag = 1;
            break;
        }
        else {
            if (Flag_PID >= 1) {   // 保证一次中断以后产生一次pid控制
                // 根据编码器累加的值，不断调整速度
                if (rightroad > leftroad + 20) {
                    Flag_PID = 0;
                    car_turn_right(80, 40);
                }
                else if (leftroad > rightroad + 20) {
                    Flag_PID = 0;
                    car_turn_right(40, 80);
                }
                else {
                    Flag_PID = 0;
                    car_turn_right(80, 80);
                }

                R_Pid_Ctrl(setspeedr, SPEED_RIGHT);
                L_Pid_Ctrl(setspeedl, SPEED_LEFT);
            }
        }
        if (Flag_Encode == 1)   //标志位检测执行 10ms 实时速度显示
        {
            char buf[256] = {0};
            // 获取当前的计数值
            int32_t lcount = -g_encoder_e1.count;   // 因为是50ms
            int32_t rcount = g_encoder_e2.count;

            sprintf(buf, "e1=%d,e2=%d\n", g_encoder_e1.count, g_encoder_e2.count);
            OLED_Clear();
            OLED_ShowString(1, 1, (u8 *)buf, 16);
            // 清空计数值
            g_encoder_e1.count = 0;
            g_encoder_e2.count = 0;
            Flag_Encode        = 0;
        }
    }

    while (1)
        ;

        // 基于速度式pid的实现
#if 0    
    
    
    // 清空pid
    PID_clear(&g_pid1);
    PID_clear(&g_pid2);

    // 设置小车为前进，速度为800
    car_forward(80); 
    // 迁移小车状态为car_state_question_1_run
    g_car_state = car_state_question_1_run;
    while (1) {
        // 读取灰度的值，并且通过灰度来判断是否检测到黑线
        if(detect_black_lines() == 1){
            // 如果检测到黑线，先关闭电机（停车）
            motor_off(&g_motor_fl);
            motor_off(&g_motor_fr);
            // 再切换为car_state_stop状态
            g_car_state = car_state_stop;
            // 最后开灯光和蜂鸣器响应
            rgb_led_set(&g_rgb_led, true, false, false);
            beep_on(&g_beep);
            break;
        }
        delay_ms(10);
    }
    delay_ms(50);
    rgb_led_set(&g_rgb_led, false, false, false);
    beep_off(&g_beep);
    
    while(1);

#endif
}

// 第二题
void on_question_2(void)
{
    // 设置为前进
    car_forward(800);
    // 切换到第一题的状态
    g_car_state = car_state_question_1_run;
    while (1) {
        // 读取灰度的值，并且通过灰度来判断是否检测到黑线
        if (detect_black_lines() == 1) {
            // 如果检测到黑线
            // 开灯光和蜂鸣器响应
            led_on();
            beep_on(&g_beep);
            break;
        }
        delay_ms(10);
    }

    // 切换状态到track
    g_car_state = car_state_question_track;

    // 关闭灯光和蜂鸣器
    led_off();
    beep_off(&g_beep);

    // 开始寻迹
    track();

    // 设置为前进
    // 切换到第二题的状态
    g_car_state = car_state_question_2_run;
    while (1) {
        // 读取灰度的值，并且通过灰度来判断是否检测到黑线
        if (detect_black_lines() == 1) {
            // 如果检测到黑线
            // 开灯光和蜂鸣器响应
            led_on();
            beep_on(&g_beep);
            break;
        }
        else {
        }
        delay_ms(10);
    }
    // 切换状态到track
    g_car_state = car_state_question_track;

    // 关闭灯光和蜂鸣器
    led_off();
    beep_off(&g_beep);

    // 开始寻迹
    track();

    // 停车
    motor_off(&g_motor_fl);
    motor_off(&g_motor_fr);
}

// 第三题
void on_question_3(void) {}

// 第四题
void on_question_4(void) {}

// 速度环控制
void car_speed_control(void)
{
    char buf[256] = {0};
    // 获取当前的计数值
    int32_t lcount = -g_encoder_e1.count;   // 因为是50ms
    int32_t rcount = g_encoder_e2.count;

    //    sprintf(buf, "e1=%d,e2=%d\n", g_encoder_e1.count, g_encoder_e2.count);
    // OLED_Clear();
    // OLED_ShowString(1, 1, buf);
    // 清空计数值
    g_encoder_e1.count = 0;
    g_encoder_e2.count = 0;
    // 计算pid，并且设置占空比
    // 根据目标速度计算出所需的compare值
    float res1 = Limit_Min_Max(PID_Calc(&g_pid1, lcount, g_target_speed1), 0, 3200);
    float res2 = Limit_Min_Max(PID_Calc(&g_pid2, rcount, g_target_speed2), 0, 3200);

    sprintf(buf,
            "%f,%f,%f,%f,%f,%f\n",
            (float)lcount,
            (float)g_target_speed1,
            (float)rcount,
            (float)g_target_speed2,
            (float)res1,
            (float)res2);
    usart_send_string(buf);

    // sprintf(buf, "res1=%d", (int)res1);

    // OLED_ShowString(2, 1, buf);

    //  sprintf(buf, "res2=%d", g_target_speed1);

    //  OLED_ShowString(3, 1, buf);
    // 根据pid算出来的速度设置compare value
    motor_set_compare_val(&g_motor_fl, res1);
    motor_set_compare_val(&g_motor_fr, res2);

#if 0  
    char buf[32];
    // 获取当前的计数值
    int32_t lcount = g_encoder_e1.count; // 因为是50ms
    int32_t rcount = g_encoder_e2.count;
    sprintf(buf, "e1=%d,e2=%d", g_encoder_e1.count, g_encoder_e2.count);
    OLED_Clear();
    OLED_ShowString(1, 1, buf);
    // 清空计数值
    g_encoder_e1.count = 0;
    g_encoder_e2.count = 0;      
    // 计算pid，并且设置占空比
    // 根据目标速度计算出所需的compare值
    float res1 = PID_Calc(&g_pid1, lcount,g_target_speed1);
    float res2 = PID_Calc(&g_pid2, rcount,g_target_speed2);
    sprintf(buf, "res1=%d", (int)res1);
    OLED_ShowString(2, 1, buf);
    sprintf(buf, "res2=%d", g_target_speed1);
    OLED_ShowString(3, 1, buf);
    // 根据pid算出来的速度设置compare value
    motor_set_compare_val(&g_motor_fl, res1);
    motor_set_compare_val(&g_motor_fr, res2);
#endif
}

// TODO: 角度环控制
void car_angle_control(void)
{
    // 获取偏航角yaw
    jy61p_get_angle(&g_angle_x, &g_angle_y, &g_angle_z);
    // float yaw = g_angle_z;//-180~+180
    float delta;

    // 计算角度差距量(自己可以去推算一下对不对)
    if (g_target_angle - g_angle_z > 180) { delta = g_angle_z - g_target_angle + 360; }
    else if (g_angle_z - g_target_angle > 180) {
        delta = g_angle_z - g_target_angle - 360;
    }
    else {
        delta = g_angle_z - g_target_angle;
    }

    if (delta > -1.5f && delta < 1.5f)   //死区，可以适当调整
    {
        PID_clear(&g_pid_angle);   // PID运算结束
        return;
    }

    // 计算pid
    float res = PID_Calc(&g_pid_angle, 0, delta);
    // 设置电机方向，电机方向自行调整，这里不一定对
    if (res > 0) {
        motor_set_direction(&g_motor_fl, motor_direction_corotation);
        motor_set_direction(&g_motor_fr, motor_direction_inversion);
    }
    else {
        motor_set_direction(&g_motor_fl, motor_direction_inversion);
        motor_set_direction(&g_motor_fr, motor_direction_corotation);
    }
    //设置结果值，还是要注意方向问题
    set_target1_speed(res);
    set_target2_speed(res);

    //不要让两个car_speed_control()同时运行
    car_speed_control();
}

// 小车停止
void car_stop(void)
{
    // 关电机
    motor_off(&g_motor_fl);
    motor_off(&g_motor_fr);
    // 清空pid
    PID_clear(&g_pid1);
    PID_clear(&g_pid2);
}

// 小车前进
void car_forward(int32_t speed)
{
    set_target1_speed(speed);
    set_target2_speed(speed);

    motor_set_compare_val(&g_motor_fl, speed);
    motor_set_compare_val(&g_motor_fr, speed);

    // 开电机
    motor_set_direction(&g_motor_fl, motor_direction_corotation);
    motor_set_direction(&g_motor_fr, motor_direction_corotation);
}

// 小车转angle度，左转是负，右转是正
void car_turn_angle(float angle)
{
    //    // 获取偏航角yaw
    //    jy61p_get_angle(&g_angle_x, &g_angle_y, &g_angle_z);
    //    float yaw = g_angle_z;
    //
    //    PID_calc()
}

// unused in main
#if 0

    // 开启蜂鸣器
//    beep_on(&g_beep);

    // 开启rgb led
//    rgb_led_set(&g_rgb_led, true, false, false);
//    delay_cycles(DELAY);   //延时0.5秒
//    rgb_led_set(&g_rgb_led, false, true, false);
//    delay_cycles(DELAY);   //延时0.5秒
//    rgb_led_set(&g_rgb_led, false, false, true);
//    delay_cycles(DELAY);   //延时0.5秒
//    rgb_led_set(&g_rgb_led, true, true, false);
//    delay_cycles(DELAY);   //延时0.5秒
//    rgb_led_set(&g_rgb_led, true, false, true);
//    delay_cycles(DELAY);   //延时0.5秒
//    rgb_led_set(&g_rgb_led, false, true, true);
//    delay_cycles(DELAY);   //延时0.5秒
//    rgb_led_set(&g_rgb_led, true, true, true);
//    delay_cycles(DELAY);   //延时0.5秒
//    rgb_led_set(&g_rgb_led, false, false, false);

    //DL_UART_disable();
//        sprintf(buf, "s1=%.2f", g_target_speed1);
//        OLED_ShowString(2, 1, buf);
//        sprintf(buf, "s2=%.2f", g_target_speed2);
//        OLED_ShowString(3, 1, buf);
        
        //        jy61p_get_acc(&g_acc_x, &g_acc_y, &g_acc_z);
        //        sprintf(buf, "ax=%.3f",  g_acc_x);
        //        OLED_ShowString(2, 1, buf);
        //        sprintf(buf, "ay=%.3f", g_acc_y);
        //        OLED_ShowString(3, 1, buf);
        //        sprintf(buf, "az=%.3f", g_acc_z);
        //        OLED_ShowString(4, 1, buf);

        //        jy61p_get_gyro(&g_gyro_x, &g_gyro_y, &g_gyro_z);
        //        sprintf(buf, "gx=%.3f",  g_gyro_x);
        //        OLED_ShowString(2, 1, buf);
        //        sprintf(buf, "gy=%.3f", g_gyro_y);
        //        OLED_ShowString(3, 1, buf);
        //        sprintf(buf, "gz=%.3f", g_gyro_z);
        //        OLED_ShowString(4, 1, buf);

//        jy61p_get_angle(&g_angle_x, &g_angle_y, &g_angle_z);
//        sprintf(buf, "ax=%.3f", g_angle_x);
//        OLED_ShowString(2, 1, buf);
//        sprintf(buf, "ay=%.3f", g_angle_y);
//        OLED_ShowString(3, 1, buf);
//        sprintf(buf, "az=%.3f", g_angle_z);
//        OLED_ShowString(4, 1, buf);

        //        jy61p_get_gyro(&g_gyro_x, &g_gyro_y, &g_gyro_z);
        //        sprintf(buf, "gx=%.1f,gy=%.1f,gz=%.1f",   g_gyro_x, g_gyro_y, g_gyro_z);
        //        OLED_ShowString(3, 1, buf);
        //
        //        jy61p_get_angle(&g_angle_x, &g_angle_y, &g_angle_z);
        //        sprintf(buf, "ax=%.1f,ay=%.1f,az=%.1f",   g_angle_x, g_angle_y, g_angle_z);
        //        OLED_ShowString(4, 1, buf);

        //        jy61p_get_acc(&g_acc_x, &g_acc_y, &g_acc_z);
        //        jy61p_get_gyro(&g_gyro_x, &g_gyro_y, &g_gyro_z);
        //        jy61p_get_angle(&g_angle_x, &g_angle_y, &g_angle_z);

        //        DEBUG_INFO("g_acc_x=%.3f, g_acc_y=%.3f, g_acc_z=%.3f", g_acc_x, g_acc_y, g_acc_z);
        //        DEBUG_INFO("g_gyro_x=%.3f, g_gyro_y=%.3f, g_gyro_z=%.3f", g_gyro_x, g_gyro_y,
        //        g_gyro_z); DEBUG_INFO("g_angle_x=%.3f, g_angle_y=%.3f, g_angle_z=%.3f", g_angle_x,
        //        g_angle_y, g_angle_z);
        //        if(g_encoder_e1.count > 1500 && g_encoder_e2.count > 1500){
//            motor_off(&g_motor_fl);
//            motor_off(&g_motor_fr);
//        }
        
//        char buf[32];
//        

//        car_speed_control();
//        
//        delay_ms(50);
#    if 0

    //    DEBUG_INFO("motor->timer = %d", (int)PWM_FRONT_MOTOR_INST);
    //    DEBUG_INFO("motor->channel = %d", GPIO_PWM_FRONT_MOTOR_C1_IDX);
    //    DL_TimerA_setCaptureCompareValue(PWM_FRONT_MOTOR_INST, 1600, GPIO_PWM_FRONT_MOTOR_C1_IDX);

    // DL_GPIO_setPins(GPIO_GRP_MOTOR_PORT, GPIO_GRP_MOTOR_PIN_FRONT_RIGHT_A_PIN);
    // DL_GPIO_clearPins(GPIO_GRP_MOTOR_PORT, GPIO_GRP_MOTOR_PIN_FRONT_RIGHT_B_PIN);



    //    OLED_Clear();
    //    OLED_ShowString(1,1,"test str", OLED_8X16);
    //    OLED_Update();
#    endif
    int i       = 0;
    g_car_state = car_state_normal_run;
    uint16_t data;
    while (1) {

        // car_run();

        DEBUG_INFO("i = %d", i++);

        DEBUG_INFO("data=%d", data);
//        jy61p_get_acc(&g_acc_x, &g_acc_y, &g_acc_z);
//        jy61p_get_gyro(&g_gyro_x, &g_gyro_y, &g_gyro_z);
//        jy61p_get_angle(&g_angle_x, &g_angle_y, &g_angle_z);

//        DEBUG_INFO("g_acc_x=%.3f, g_acc_y=%.3f, g_acc_z=%.3f", g_acc_x, g_acc_y, g_acc_z);
//        DEBUG_INFO("g_gyro_x=%.3f, g_gyro_y=%.3f, g_gyro_z=%.3f", g_gyro_x, g_gyro_y, g_gyro_z);
//        DEBUG_INFO("g_angle_x=%.3f, g_angle_y=%.3f, g_angle_z=%.3f", g_angle_x, g_angle_y, g_angle_z);
        delay_cycles(DELAY);   //延时0.5秒
        // DEBUG_INFO("e1.count = %d", g_encoder_e1.count);
        // DEBUG_INFO("e2.count = %d", g_encoder_e2.count);

        // DL_GPIO_togglePins(GPIO_GRP_BEEP_PORT,GPIO_GRP_BEEP_PIN_BEEP_PIN);//电平翻转驱动LED
        // DL_GPIO_togglePins(PORTB_PORT,PORTB_RGB_R_PIN | PORTB_RGB_G_PIN |
        // PORTB_RGB_B_PIN);//电平翻转驱动RGB
    }
#endif

// unused
#if 0

//void pid_speed_control(motor_t* motor, motor_encoder_t* encoder, pid_incremental_t* pid,
//                       float target)
//{
//    // 1.设定目标速度
//    // 2.获取当前速度
//    // 3.输入PID控制器进行计算（注意：电机输出限幅）
//    // 4.PID的输出值，输入给电机
//    //float duty     = pid_incremental_calculate(pid, target, encoder->count);
//    //encoder->count = 0;
//    //motor_set_duty(motor, duty);

//    //
//    //	if(motorA_dir){
//    //        motorA.now = Encoder_count1;
//    //    }
//    //    else{
//    //        motorA.now = -Encoder_count1;
//    //    }
//    //	if(motorB_dir){
//    // motorB.now = Encoder_count2;
//    //}
//    //    else{
//    // motorB.now = -Encoder_count2;
//    //}
//    //	Encoder_count1 = 0;
//    //	Encoder_count2 = 0;
//    //
//    //	pid_cal(&motorA);
//    //	pid_cal(&motorB);
//    //	//
//    //	pidout_limit(&motorA);
//    //	pidout_limit(&motorB);
//    //
//    //	motorA_duty(motorA.out);
//    //	motorB_duty(motorB.out);

//    //	datavision_send();
//}

void pid_angle_control(float angle) {
    // 1. 设定目标角度
    // 2. 获取当前角度
    // 3. 将当前值和目标值进行位置式pid计算
    // 4. 设置单电机目标速度，将角度环的输出设置为速度环的目标值（角度环的输出没有占空比限制）

}

#endif

//
